#ifndef ASTAR_PLANNING_DEBUG_H
#define ASTAR_PLANNING_DEBUG_H

#define __APP_NAME_ "astar_planning_debug"

#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "geometry_msgs/Point.h"
#include <nav_msgs/OccupancyGrid.h>
#include "nav_msgs/Path.h"
#include "AstarNode.h"
#include <algorithm>

class AstarPlanning{
    public:
        AstarPlanning();
        ~AstarPlanning();

    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
    
    private:
        std::string input_map_topic_;
        std::string input_start_topic_;
        std::string input_goal_topic_;
        std::string output_path_topic_;
        std::string output_mask_topic_;
        std::string output_nav_path_topic_;
    
    private:
        double inflate;
    
    private:
        bool Euclidean;
    
    private:
        std::shared_ptr<nav_msgs::OccupancyGrid> cost_map_ptr_;
        std::shared_ptr<nav_msgs::OccupancyGrid> mask_map_ptr_;
        std::shared_ptr<geometry_msgs::PoseWithCovarianceStamped> start_pose_ptr_;
        std::shared_ptr<geometry_msgs::PoseStamped> goal_pose_ptr_;
    
    private:
        bool costmap_ready = false;
        bool start_ready = false;
        bool goal_ready = false;
    
    private:
        ros::Subscriber costmap_sub_;
        ros::Subscriber goal_sub_;
        ros::Subscriber start_sub_;
        ros::Publisher mask_costmap_pub_;
        ros::Publisher nav_path_pub_;
    
    private: 
        void start_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
        void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
        void costmap_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
        nav_msgs::OccupancyGrid mask(nav_msgs::OccupancyGrid & costmap);
        int coordinate_2_index(double x, double y);
        std::vector<int> coordinate_2_mxy(double x, double y);
        geometry_msgs::Point index_2_coordinate(int index);
        std::vector<int> index_2_mxy(int index);
        int mxy_2_index(int mx, int my);

        /* 判断该点是否在list中 */
        bool is_in_nodeList(int index, std::vector<AstarNode>& nodeList);

        /* 寻找openlist中代价最小的Node */
        AstarNode get_min_f_node(std::vector<AstarNode>& openList);
};

#endif